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ABSTRACT 


This  Is  one  of  a  series  of  tutorial  reports  deriving  the  discrete 
recursive  Kalman  estimation  equations.  The  series  proceeds  from  unweighted 
to  weighted  least  squares  parameter  estimation  in  a  vector  space  setting  to 
the  stage-wise  updating,  to  time  varying  parameter  or  state  variable  esti¬ 
mation.  The  derivations  have  been  stage-wise  tutorial  in  an  attempt  to  make 
the  theory  accessible  to  the  "non-specialist"  in  optimization  theory. 
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TKTTorvntTrTTnKi 


Modem  state-vector  recursive  estimation  theory  (in  the  sense  of  Kalman) 
has  been  derived  from  many  viewpoints:  conditional  probability,  orthogonal 
projections,  least  squares,  minimum  variance,  maximum  likelihood,  etc.  The 
individual  user  of  the  theory  may  understand  the  derivation  of  the  equations 
obtained  by  any  one  of  the  above  theories  or  possibly  by  a  number  of  approaches 
depending  upon  the  individuals  background. 

This  report  is  the  third  of  a  tutorial  series  deriving  the  estimation 
aquations  in  a  vector  space  weighted  lease  squares  setting.  The  essential 
areas  for  understanding  the  theory  via  this  approach  are: 

1.  Unweighted  Least  Squares  Parameter-Vector  Estimation  and  the 
Variance-of-the-Estimate  Matrix. 

2.  Discrete  Matrix  Recursive  Methods  apply  to  (1)  for  Real  Time 
(on  line)  Computer. Processing. 

3.  Weighted  Least  Squares  Parameter  Estimation  and  Variance-of-the- 
Estimate  Matrix  for  Correlated  Holse. 

4.  Discrete  Matrix  Recursive  Methods  applied  to  (3)  for  Real  Time 
Computer  Processing. 

5.  Recursive  Weighted  Least  Squares  State-Vector  Estimation  Theory 
(Kalman  Theory). 

Items  1  and  2. are  completed  and  published  in  Reference  4.  Item  3  is 
completed  and  published  in  Reference  6.  Item  4  is  pot  completed  yet. 

Item  5  is  the  contents  of'  the  current  report. 

This  report  is  restricted  to  full-rank  matrices  so  that  the  reader  may 
understand  the  basic  principles  of  the  theory.  Mon-full  rank  matrices  processed 
recursively  require  a  knowledge  and  background  in  Psuedo  Inverses ,  and  many  more 
sophisticated  linear  algebra  concepts.  The  minimization  criterion  is  derived  by 
taking  the  partial  derivative  of  a  scalar  with  respect  to  a  pxk  weighting  matirx- 
the  classical  gradient  approach. 

The  derivations  can  be  achieved  completely  algebraiclv  in  terms  of  orthogonal 
projections  relating  to  spaces  and  sub-spaces.  Many  sophisticated  paper  exist  In 
the  literature  using  this  approach.  The  algebraic  approach  may  not  be  as  readily 
accessible  to  the  engineer  trained  in  classical  mathematics. 


NOTATION 


The  notations  used  in  the  report  is  an  effort  to  blend  the  notation 
of  Friedman  fnr  inner-products  and  dyadic  products  with  the  current 
journal-literature  on  vector-spaces,  psuedo-inverses ,  state-vectors,  etc. 


X, 

pxk 


Capital  letters  designate  matrices  of  size  p  rows  and 
k  columns. 


when  p  **  1,  the  matrix  is  called  a  column  vector,  and 
we  use  Friedmans  symbol  to  distinguish,  this  matrix. 


when  k  *  1,  the  matrix  is  a  row  vector  of  dimension  p, 

"inner-product"  or  scalar  product. of  two  vectors. 

"outer-product"  or  dyadic  product  of  two  vectors. 

. xfo-j.  Matrix  X  partitioned  into  a  row  k-tuple  of 
/*  column  vectors  from  a  p-space. 


Matrix  X  partitioned  into  a  p-column  tuple  of  row 
vectors  from  a  k-space 


x  small  x  is  a  scalar 

x*  scalar  from  a  column  vector 

Xj  scalar  from  a  row  vector 

Scalar  here  is  a  "real  field"  element. 
Consider  the  system  of  two  vector  equations 


where : 
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x(k+l)( 


are  p-dimensional  column  vectors  describing 
the  states  at  stage  k  and  stage  k+1. 


*(k+l,  k) 


Is  a  pxp  state  transition  matrix. 


is  a  p-diiaeutsiunal  deterministic  forcing  vector  tor 
which  we  can  write  a  vector  function. 

is  a  p-dimensional  uncertainty  or  noiae  vector,  it  is 
the  composite  of  the  random  noises  and  the  variables 
we  fail  to  model. 

is  the  m-dimensional  observation  vector,  m  is  less 
than  or  equal  to  p. 


is  the  known  matrix  describing  how  the  state  vector  is 
functionally  related  to  the  observation  vector  (if  the 
instruments  were  noise  free) . 

is  an  m-dimensional  additive  instrument  noise  vector. 
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I  .  ONE  PARAMETER  FAMILY  OF  DISCRETE  TRAJECTORIES 


This  section  considers  the  one  parameter  family  of  i  trajectories 
generated  by  i  different  values  of  the  initial  condition  parameter  x^(l). 
Each  of  the  i  trajectories  consists  of  a  sequence  of  k  different  points 
or  stages. 

We  consider 

(1)  The  homogeneous  case 

(2)  The  deterministically  forced  case. 

Homogeneous  System 

Consider  the  system  of  scalar  equations 


x(k+l)  *(j?(k+l,  k)x;(k)  +  f(k)  ♦  u(k)  (1) 

z(k)  *  h(k)x(k)  ♦  v(k) 
where  the  following  constraints  apply 

f(k)  «  u(k)  *  v(k)  *  0  (3) 

h(k)  ■  hQ  a  constant  (4) 

$(k+l,k)  =  a  constant  (5) 

then  equation  )  and  (2)  become 

x(k+l)  *  <J>0  x(k)  (6) 

z(k)  =  hQ  x(k)  (7) 

If  further,  we  consider  the' measurement  to  be  exact 

z(k)  »  x(k)  '  (8) 


or 


then  we  need  only  consider 


x(k+l)  *  403c ( k ) 


(9) 

(10) 


1 


For  anv  cnnstflLnt  ^  ® ill 


x(2)  m<pQ  x(i) 


x(3)  x(2)  *<j>6(fa  x(l))  *fQ  *(1) 


x(k)  *^q  1  x(l). 

If  <fi  is  less  than  one.the  trajectory  of  Equation  (  Lfor  a  given  initial 
condition  decreases,  for  example  ' 

<t>0  »  1/2 

x(2)  -  \  x( 1 ) 

x(3)  »  (|)2  x(l)  -  Jx(l)  (18) 

x(l»)  *  £  x(l) 

4 

x (k )  *  (I)11'1  x(l) 

The  sequence  of  k  vectors  are 


1  x(l) 

2 


l  x(i) 

TT 


which  plot  as  shown  in  Fig.  (  3  ) 


iiiiliiBin 

■■■■■■■■■■■ 

■■■■■■■■■■■ 

RSB 


Fig.  (  3  )  Contracting  or  Decaying  Sequence  bQ  *  1/2 


The  first  noisy  observation  is 

z  (1)  *  h(l)  x,(l)  +  v  (1) 
jn  j  n 

Using  ( 3  )  and  ( 6  )  in  (  4 ) 

7^(1,  0)  =  hdHxjfl)  -  XjCL,  0)]  +  vn(i). 

Using  (7)  in  (5) 

Xjn(l,  1)  ■  Xj(l,  0)  +  w(l)h(l)[Xj(l)  -  ^Cj(l,  0)  ]  +  v(l)vnU) 

*  [1  -  w(l)h(l)]  Xj(l,  u)  +  v(l)h(l)Xj (l)  +  w(l)  vn(l). 

We  nov  define  tvo  new  errors  as  differences  of  the  previous  variables. 
Xj(l)  -  x'jU,  0)  *  Sj(l.  0) 


Xj(l)  -  Xjn(l,  1)  ■  xJn(l,  1).  C 

The  variable  x.(l)  is  the  actual  (but  unknown)  process  state  variable  that 
exists  on  trajectory  J.  The  variable  1^n(l^l)  is  the  estimate  of  Xj(l), 

having  used  the  first  noisy  observation. 

Hence  x.  (l,  1)  is  the  error  in  our  estimate  of  the  state  at  stage  1  based 
on  one  measurement. 

From  equation  (j*  )  we  see  that  if  our  initial  guess  ^c,(l,  0)  is  wrong,  then 
z  (1,  0)  will  also  be  wrong  and  after  our  observation  z(l)  comes  in  and 
onr  computed  error  (1,  0)  of  equation  (4)  is  large,  then  by  equation  (5) 

J  f-uf 

we  willvhave  a  correction  term  which  is  the  product  (w(l)Zj(l,  l))  of  a 

large  term  times  the  weight  value  w(l). 

Continuing  with  the  derivation  of  the  weights,  we  obtain  the  error  term  by 
equation  (  3)  in  equation  (11  )  as 

*x^n<l,  l)  =  Xj(l)  -  Xj(i,  1)  =  Xj (l)-w(l)h(l)x^(l)  ( 

-  [ (l-w(l)h(l)]  Xj(l,  0)  -w(l)vn(l). 


(1,  l)  *  (l-w(l)h(l) ] [x, (l)  -  x  (l,  0)]  -  w(l)v  (1). 
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Using  equation  (9  )  in  equation  (12) 


x.  (i,l)  -  [i  —  w (x)li(i ) ]  x.d,  0;  -  wix)  v  ( X ) .  (13) 

jn  J  n 

If  we  square  the  error  of  equation  (13) 

xjn(lt  1)  =  [l-v(l)h(l)]%2(l,  0)  -  2v(l)vn(l)[l-w(l)h(l)]^(l,  0)  (14) 
+  V2(l)vn2(l). 

If  we  take  the  partial  derivative  of  equation  (14)  with  respect  to  w(l)  and 
equaifc  to  zero  we  obtain 


^  x]jn(l,  1)  =  2[1  -  w(l)h(l)]  (-h(l))  x2(l,  0) 
^IKT)  _  2  (!)[]_  _  w(l)h(l)  lx.(l.  0) 


-  2vn(l)[l  -  w(l)h(l)]xj(l,  0) 
+  2w(l)  v2(l)  =  0 


Taking  the  expected  value  over  all  experiments  in  which  J  and  n  are  varying 
we  obtain  - 

ps2  a,»n  <  o  -) 

* -ti  -  w(i)h(D]  E^(i,o)jh(i)  (i6) 


+  w( 1 )  E 


H'11}  - 


Define  the  variances 


eK(1-  °>] 

E[vn2(i)}  -cr„(i) 


C&J  1,  0)  =  p(i,  o) 


We  follow  a  number  of  current  Kalman  papers  and  papers  about  Kalman 
Estimation  and  designate  the  variance  in  state  as  p(l,  0). 

The  cross  term  of  equation  (!’)  under  the  expectation  operator  is  zero 
when  the  following  conditions  hold, 


a£[l  -  w(l)h(l)]  vn(2)  Xj(l,  0)J 

*  [l  -  w(l)h(l) ]  E  £vn(l)  Xj(l,  0)^ 


We  now  assume  that  our  initial  guess  of  x(l,0)  ia independent  of  vfi(l)  as 
n  ranges  over  all  allowable  values,  that  is 


jynijU,  o)^. 


Under  these  assumptions  equation  (16)  and  (18)  become 


0  -  -.[1  -  w(l)h(l)]h(l)  p(l,  0)  +  vCUCJJyd) 


-h(l)p(l,  0)  w(l)h2(l)  p(l,  0)  +  w(l)f  (i)  =  0 


w(l)[n  (l)  p(lt  0)  +ffi^(l)3  °  h(l)  p(l,  0) 
w(l)  *  h(l)  p(l,  0)[h2(l)  p(l,  0)  +0^0(1)]_1 
The  value  of  p(l,  0)  which  by  equation  (17)  is 


p(l,  0)  -  e/[Xj(1)  -  Xj(l,  0)]2 

_ j 


guess 


can  he  obtained  by  guess  or  by  a  "learning  process",  or  by  a-priori 
knowledge  about  the  system. 

The  variance  of  the  estimate  of  state  at  stage  1  based  on  the  observation 
at  stage  one  can  alBo  be  obtained  by  equation  ( 14)  as 

E£*jnd*  1)}  *  [1  “  v<l)h(l)]2  °>]  +  w2(l)  E^vn(1^  (26^ 

when  the  cross  terms  and  zero,  that  is 

eJvq(1)  Xj(1,  0)J  2w(l)[l  -  v(l)h(l)]  =  0.  (27 

The  variance  terms  of  equation  (  )  will  be  denoted  as 


O&ti.  1)  -  1)^2  p  (1,  1) 


The  p(l,  l)  designation  is  in  keeping  with  the  now  near  classical 
notation. 


Expanding  the  terms  of  equation  (26) 

p(l,  1)  *  [1  -  w(l)h(l)]2  p(l,  0)  +  W2(l UT^l)  (30) 

■  [1  -  2w(l)h(l)  +  w2(l)h2(l) ]  p(l,  0)  +  v2(l)  flj^(l) 

-  p(l,  0)  -  2w(l)h(l)  p(l,  0) 

+  W2(l)[h2(l)  p(l,  0}  +0“  (l)]. 
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Consider  the  last  term  in  equation  (30)  and  use  equation  (24)  for 
v2(l),  then 

v2(i)[h2(i)  P(i,  o)  (3D 

»ir 

»  h2(l)  P2(l,  0)[h2(l)  p(l,  0)  +(^„(l)r2  [h2(l)  p(l,  o)  +^0(D] 

«  h2(l)  P2(l,  0)(h2(l)  p(l,  0)  +^(1)]_1 

-  h(i)  pd,  o)  £h(i)  pd,  o)  [h2(i)  pd,  o)  +07V,(1)]"1 

Replacing  the  bracket  terra  of  equation  (31)  by  v(l)  in  equation  (24) 
we  obtain 

w2(l)(h2(l)  p(l,  0)  +0^(1)]  ■  h(l)  p(l,  0)  v(l)  (32) 

Using  (  32)  in  -equation  (30)  ve  obtain 

p(l,  1)  •  p(l,  0)  -2vr(l)  h(l)  p(l,  0}  +  h(l)  p(l,  0)  w(l)  (33) 

»  p(l,  0)  -  w(l)  h(l)  p(l,  0). 

P(i,  l)  -  p(i,  o)[i  -  vd)h(i)] 

also  using  equation  (24)  in  equation  (33)  ve  can  write  p(l,  l)  as 

p(l,  1)  ■  p(l,  0)  -  p(l,  0)h(l)[h(l)p(l,  0)h(l)  +^rv(l)]'1p(l,0)h(l) 

Equation  (34)  is  the  scalar  variance  (one  dimensional  uncertainty  ellipsoid) 
of  the  estimate  of  the  state  using  till  of  the  current  observations. 

We  can  now  predict  what  the  next  state  variable  value  should  be  by  pro¬ 
rogating  forward  with  the  known  dynamics  as 


^(2,  1)  »<f>(2,l)  x(l,  1)  +  f(l). 

(35) 

The  prediction  of  the  next  observation  is 

z(2,  1)  «  h(2)  "x(2,l) . 

(36) 

The  elliposoid  of  uncertainty  of  the  predicted  state  is 

p(2,  1)  =  e|V(2,  1)  7 

(37) 

where 

~(2,  1)  =  x(2)  -^(2,  1) 

(38) 

(34) 
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Using  equation  (1)  and  equation  (35)  in  equation  (38) 

/x(2,l)  =4)(2.  1)  xU.  1)  '+  u(l)  (39) 

if  (2,  1)  -<jX2,  1)  x2(l,  l)(J>(2,  1)  +  2^(2,  1)  £(l,  1)  u(l)  +  u2(l) 

The  expected  value  over  all  experiments  of  equation  (40)  is 

l)j  «  p{2,  1)  -<^(2,  1)  p(l,  l)(j)(2„  1)  +0^u(  1)  (41) 

where 

CTm(1>  ■  <«) 

Cross  term  statistical  independence  assumptions  are 

ifcl.  1)  u(ljj  -  0.  (43) 

etc. 

In  conplusion,  at  stage  one,  we  do: 

Given 

guess  or  estimate 

p(l,  0)  and  x(l,  0) 
compute 


5(1,  0)  =  h(l)  x(l,  0)  (44) 

Measure  and  compute  error 

Tu,  0)  «  z(l)  -  *(l,  0)  (45) 

Compute  weight  w(l)  equation  (  ) 

v(  1 )  -  h(l)  p(l,  0)(h(l)p(l,  0)h(l)  +  CT(l)]"1.  (46) 

Update  or  correct  state-estimate 

5(1.  1)  -5(1,  o)  +  w(l)lf{l,  0)  (47) 

Compute  ellipsoid  of  uncertainty  of  state  estimation  (48) 

p(l,  1)  «  P(1,0)[1  -  v(l)h(l)] 


(40) 


20 


Predict  or  propogate  one  stage  into  future  with  known  dynamics  the  following 
three  variables : 


Predict  next  stage. 


A- 

X) 


,  /  rx  .  \  .  .  \  -  /  -  \ 

x/  x v x |  x;  r  i\,x) 


t*V) 


Predict  next  observation. 

z(2,  1)  -  h(2)  x(2,  1) 

Predict  ellipsoid  of  uncertainty  in  state 

p(2,  1)  -  *(2,  1)  p(l,  1)  *(2,  1)  +  0UU(1). 

Wait  for  stage  k  *  2  and  second  observation  to  come  in. 

Stage  k  »  2. 

The  second  observation  is  taken  at  stage  2  and  is 

z.  (2)  -  h(2)  x..  (2)  ♦  v  (2) 

Jn  d  a 

where,  as  before,  z,  (2)  is  known,  but  x.  (2)  and  v  (2)  are  unknown, 
jn  j  n 

We  compute  the  error  signal 


(50) 


(51) 


(52) 


V2’  1} 


zjn(2)  -  ZJn(2*  1}  ' 


(53) 


The  sub-scripts  J,  n  can  now  be  dropped  for  simplicity  of  notation,  where 
it  1b  henceforth  understood  that x(2,  l)  for  example  means  the  best 
estimate  of  the  true  ,1th  trajectory  based  on  the  sequence  of  instrument 
noises  ^v  . 
n 

Using  equation  (50)  and  equation  (52)  in  equation  (53) 


z(2,  1)  =  h(2)[x(2)  -  x(2,  1)]  +  v<2) 


(54) 


we  now  have  computed  the  error  in  the  observation,' hence  we  can  correct 
our  estimate  of  what  the  state  variable  should  be  based,  on  the  use  of  the 
second  observation,  that  is 

x(£,  2)  -  x(2,  1)  +  w(2)  /z(2,  1).  (55) 

Ab  before,  we  now  seek  the  "feed-back"  or  correction' weight  v(2)  at  stage 
2.  Before  deriving  the  v(2)  expression,  use  (54)  in  (55) 

x(2,  2)  «  x(2,  1)  +  w(2)h(2)[x(2)  -  x(2,  l)J  +  w(2)  v(2)  (56) 

x(2,  2)  -  (1  -  w(2)h(2)]  *5(2,  l)  +  v(2)  h(2)  x(2)  +  v(2)  v(2).  (57) 
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Prom  the  error  in  state  estimation  at  stage  2  using  the  second  observation 
as  before 

£(2,2)  »  x(2)  -  x(2,  2)  »[l— w(2)  h(2)]x(2,l)  +  v(2)  v(2)  (58) 

Multiply  equation  (58)  by  itself  to  obtain  the  Bquare  of  the  error  tera  as 
/x2(2,  2)  ■  tl  -  v(2)  h(2))2xe(2,  1)  (59) 

♦  2v(2)  v(2)[l  -  v(2)  h(2)]  x(2,  l) 

+  w2(2)  v2 ( 2 ) . 

Observe  that  the  error  and  the  square  of  the  error  in  equation  ( 59)  is  a 
function  of  v(2). 


In  order  to  select  a  v(2)  which  will  minimize  the  square  of  the  error  we 
take  the  partial  derivative  of  equation  (59)  with  respect  to  v(2)  and 
equate  this  "gradient"  term  to  zero,  hence 

"  -2h(2)tl  "  w(2)  h(2)1  *2^2*  l>-  (60) 

♦  2v( 2 ) [ 1  -  w(2)h(2)]  x(2,  1) 

♦  2w(2)  v(2)  (-  h(2))  x(2,  1) 

+  2w(2)  v2(2)  ■  0 


Since  we  want  v(2)  to  be  the  same  regardless  of  how  many  times  ve  repeat 
the  experiment;  or,  from  a  "single-test"  stand‘-point~,  w(2)  should  be 
selected  to  hold  regardlees  of  the  unknown  sequence  driving  the  tvo 
variables  at  this  stage  u.(2)  and  v  (2).  Consequently  ve  take  the  ex- 

J  n  V, 

pected  value  over  all  admissible  vectors  ^Ju  and  and  obtain 

"  W(2)  h(2)3  (-h<2>)p(2»  I’)’*  v(2)ovy(2)  *  0.  (61) 

The  assumption  of  statistical  independence  of  the  variables  x( 2,  l)  and 
v(2)  is  again  assumed 


E  (x(2,  1)  v(2))  *  0 


(62) 

(63) 
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The  assumptions  of  equation  (63)  can  be  relaxed  but  then  one  needs  a 
deeper  knowledge  of  multi-linear  algebras.  matrix-Dackagine  and  partitioning 
matrix  psuedo- inverse ,  etc.  Hence  such  "highly-colored"  or  correlated 


noise  cases  will  not  be  discussed  in  this  paper.  The  majority  of  published 


papers  on  Kalman  theory  malic  the  impl 
equation  (63)  for  arbitrary  large  k. 


led  Gauss iau  assumptions  implied  by 
Computer  storage  problems  for 


correlated  noise  for  large  k  are-also  a  problem. 


Solving  equation  (60)  for  w(2) 


w(2)[h(2)  p(2,  l)  h(2)  +  <VJ2)]  -  h(2)  p(2,  1) 


(64) 


The  second  weight  can  be  computed  since  h(fl),  QV (2)  are  assumed  known  and 
p(2,  l)  was  computed  at  stage  1. 


We  can  nov  derive  the  expression  for  p(2,  2),  using  equation  (65)  and 
taking  the  expected  value  over  all  experiments 

p(2,  2)  -  E  {S2( 2,  2)>  -  [1  -  w(2)h(2)]2  p(2,  l)  +  v2(2)o  (2),  (66) 

with  assumptions 

E  (v(2)  x(2,  1)}  *  0.  (67) 

Expanding  equation  (66) 

p(2,  2)  *  [1  -  2w(2)  h(2)  +  w2(2)  h2(2> ]  p(2,  l)  ♦  v2(2)ayy(2)  (68) 


p(2,  2)  =  p(2,  .1)  -  2v(2)  h(2)  p(2,  l)  (69) 

+  w2(2)  [h(2)  p(2,  1)  h(2)  +  o^(2)] 

Consider  the  last  term  of  equation  (69)  using  equation  (65)  for  w(2) 

v2(2)  [h(2)  p(2,  1)  h(2)  +  0yy(2)]  (70) 

«h2(2)  p2(2,  l)[h2(2)  p(2,  1)  +  0vv(2)r[h2(2)p(2,l)  +  0^(2)] 

»  h2(2)  p2(2,  1)  [h2(2)  p(2,  1)  +  ovv(2)]"1 
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and  by  equation  (65)  for  v(2)  we  obtain 

/  \  fl-  /  n\  ..  /  A  A  \  t  /  a\  *a>  /  •  I  1  .  i  t  t  » 

-  x,  IU«=y  =  nvz;  pvz,  i)  via;  (71) 

Using  equation  ( 71 )  in  equation  (69) 

p(2,2)  «  p(2,  1)  -  2v(2)  h(2)  p(2,  l)  +  h(2)  p(2,  l)  v(2)  (72) 

or 

p(2,2)  -  p(2,  1)  -  p(2,l)  h(2)  v(2)  (73) 

p(2,2)  -  p(2,  1)  [1  -  w(2)  h(2))  (74) 

Equation  (73)  can  also  be  written  as  by  equation  (65)  in  equation  (73) 

p(2,  2)  -  p(2,  1)  -  p(2,  1)  [h2(2)p(2,l)  +^T(2)]“1  h(2)p(2,  l)  (75) 

We  can  now  predict  at  stage  k  ■  3  the  state 

*(3,  2)  «  #(3,  2)  x(2,2)  +  f(2)  (78) 

and  the  observation 

*(3,  2)  «  h(3)  x(3,  2)  (77) 

and  the  state-variance  tern 

P( 3,  2)  -  E  2)}.  (78) 

By  equation  (l)  and  equation  (76) 

x(3)  ■  ♦(3,2)  x(2)  +  f(2)  +  u(2)  (79) 

*( 3,2)  ■  4(3,  2)  x(2,  2)  f(2)  (80) 

and  subtracting 

x(3)  -  x(3,  2)  ■  *x(3,  2)  ■  $(3,  2 )  *x(2,  2)  +  u(2)  (81) 

Squaring  equation  (si) 

T*(3,  2)  «<t>(3,  2)'22(2,  2)4>(3<  2)  (82) 

♦  2^(3,  2)  7(2,  2)  u(2) 

♦  u2(2). 
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Taking  the  expected  value 

E  {>^(3.  2)}  =  d>(3.  2)  Enf.2(2.  2H  4(3.  ?)  +  V.  (»2(o)} 
or 

p(3,  2)  =  4.(3,  2)  p(2,2)  4(3,  2)  +  ouu(2) 

Summarizing  the  steps  at  stage  k  *  2,  then  we; 
measure  and  compute  error 

^(2,  l)  -  z(2)  -  z(2,  1) 
compute  weight  v(2)  by  equation  (65) 

v(2)  «  h(2)  p(2,  l)(h(2)  p(2,  1)  h(2)  ♦  tf^)]"1 
compute  corrected  estimate  of  state 
x(2,  2)  -  x(2,  1)  +  v(2)  ^(2,  1) 
compute  variance  of  state  equation  (74) 

p(2,  2)  -  p(2,  1)  -  v(2)  h(2)  p(2,  1) 

Predict  (update)  state  via  dynamics  equation  (80) 
x(3,  2)  =  4(3,  2)  x(2,  2)  +  f(2) 

Predict  observation  at  next  stage 
z(3,  2)  -  h ( 3 )  x(3,  2) 

Predict  next  stage  state-variance 

p(3,  2)  =  4(3,  2)  p(2„2)  4(3,  2)  +  ouu(2) 
wait  for  next  stage  or  third  observation  to  arrive. 


(84) 

(85) 

(86) 

(87) 

(88) 

(89) 

(90) 

(91) 


Stage  ko 

The  derivations  of  the  equations  will  not  be  repeated  for  stage  k,  the 
relations  will  be  based  on  the  mathematical  process  of  reasoning  by  analogy. 
The  treatment  of  the  multi-variable  or  vector  case  will  derive  the  relations 
at  stage  k,  but  will  not  develop  the  stage-wise  logic  at  k  =  1  and  k  ■  2. 

We  have  available  from  previous  stage  predictions 

x(k,  k-l) 

^(k,  k-l) 
p(k,  k-l) 
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and  stored  o^!k),  ouu(k) 

Measure  and  compute  error 

Y(k,  k-l)  ■  z(k)  -  z'(k,  k-l)  (92) 

Compute  w(k) 

w(k)  -  h(k)  p(k,  k-l)[h2(k)  p(k,  k-l)  ♦  o^(k)]-1  (93) 

Compute  corrected  state  estimate 

^c(k,  k)  »^t(k,  k-l)  +  v(k)#z(k,  k-l)  (94) 

Compute  variance 

p(k,  k)  -  p(k,  k-l)  [1  -  w(k)  h(k)]  (95) 

Predict  next  state 

x(k  ♦  1,  k)  ■  4(k  +  1,  k)  x(k,  k)  +  f(k)  (96) 

Predict  next  observation 

*<k  +  1,  k)  -  h(k+l)  x(k+l,  k)  (9?) 

Predict  variance  of  state 

p(k+l,  k)  *  4(k+l,  k)  p(k,  k)  4(k+l,  k)  +  ®uu(k).  (98) 

We  define  the  noise  variances  in  the  notation  of  the  many  Kalman 
oriented  papers,  that  is 

>* 

0^u(k)  ■  q(k)  (99) 

0^(k)  -  r(k)  (100) 

The  three  familiar  equations  can  be  written  as 

x(k+l,  k+1)  -  d(k+l,  kj'xU,  k)  (101) 

+  p(k+l,  k)  h(k+l)[h(k+l)  p(k+l,  k)  h(k+l)  ♦  r(k)]”1 
[z(k+l)  -  h(k+l)  ♦(k+l,  k)  x(k,  k)] 
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p(k+l,  k)  =  $(k+l,  k)  p(k,  k)  4 ( k+l ,  k)  +  q(k)  HO, 

p(k+l ,  k+l)  =  p(k+.l,  k)  -  p(k+l,  k)  h(k+l)  (10 

[h(k+l)  p(k+l,  k)  h(k+l)  +  r(k)]h(k+l)  p(k+l,  k) 

We  shall  now  define  In  words  the  meanings  at  stage  k  of  the-  variables  and 
rewrite  the  equations  using  the  distinguishing  j  and  n. 

Xj(k+1)  -^>(k+l,  k)  Xj(k)  +  f(k)  +  u^(k)  (10' 

Zjn(k)  »  h(k)  x^(k)  +  vn(k)  (10 

xJn(k+l,  k+l)  =(J>(k+l,  k)  x^Ck,  *) 

+  p(k+l,k)h(k)[h2(k+l)  p(k+l)  +  r(k)]”1 

t*jn(k+l)  -  h(k+l)<|>(k+l,k)  ^n(k,  k)]  (10 

p(k+l,  k)  «<|)(k+l,  k)  p(k,  k)(J)(k+l,  k)  (10 

p(k+l,  k+l)  ■  p(k+l,  k)  -  p(k+l ,  k)  h(k+l)  (10 

lh2(k+l)  p(k+l,  k)  +  r(k)]-1h(k+l)  p(k+l,  k) 

x(k)  *  x . (k)  is  the  true  (unknown)  value  of  the  process  state  at 
2  stage  k  as  a  result  of  the  unknowns  u.(l),  ...  u.(k) 
forcing  the  system.  J 

z,  (k)  •  z,  (k)  is  measurement  of  the  true  noise  process  x,(k)  with 

J  J  additive  unknown  measurement  noise  v  (k).  J 

n 

A  ’  A 

x  (k,  k)  «=  x(k,  k)  is  the  best  estimate  of  the  state  at  stage  k  of 
J  the  Jth  trajectory  based  on  past  observations 

up  to  stage  k,  that  is  recursively  we  have  used 
noisy 

zJn(l)»  zJn(2)  **•  zjn(k) 
made  noisy  by  v^d),  •  ••  un(k). 

A  .  .  A 

x,  (k+l,  k)  ■  x(k+l,  k)  is  the  best  estimate  of  the  state  of  the  Jth 
J  trajectory  at  stage  k+l,  based  on  observations 

only  up  to  k.  Also  interpreted  as  the  predic¬ 
tion  of  the  state  at  next  stage  k+l,  based  on 
current  stage  k  and  past  measurements. 
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VECTOR  ESTIMATION  EQUATIONS 


This  section  derives  the  Kalman  Estimation  Equations  for  the  multivariable 
case  using  matrix  analysis  methods .  The  derivation  techniques  are  the  seme 
as  used  in  the  previous  scalar  case.  The  essential  difference  lies  in  the 
minimisation  methods.  The  variance  of  the  estimate  in  state'  for  the  scalar 
case  is  a  scalar  valued  function  of  a  scalar  argument  v(k).  For  the  vector 
case,  the  trace  of  the  variance  matrix  of  the  estimate  of  state  is  like¬ 
wise  a  scalar-valued  function,  but  a  function  of  a  matrix  of  p  rows  and  fy\ 
columns  Wvk) .  The  minimization  of  a  scalar-valued  function  with  respect 
to  ttys  matrix. 

One  can  arrive  at  the  equations  via  strictly  algebraic  concepts  of  orthogonal 
projection  matrices  etc.,  in  which  one  does  not  have  to  enter  into  discussions 
of  partial  derivatieB,  continuity  of  continous  variables  and  gradients. 

Since  the  majority  of  expected  readers  are  as summed  to  be  more  familiar  with 
'the  least-squares  criterion  via  gradients,  this  report  will  stick  strictly 
with  this  method. 

The  general  linearized  vector  equations  are 

x(k+lKj>*  §(k+l,  k)  x(kmj>+  B(k)f(kH£>  +  N(k)u(k)fe>  (l  ) 

/  /  pxg  /  pxq 

a(k)^rjj>  »  H(kl  x(k )^>  +  v(k)^B>  (2) 

The  deterministic  k-varying  vector  i'tk)Q>  is  of  dimension^g  less  than  or 
equal  to  p,  and  gets  distributed  or  crats-coupled  into  till  p  state 
variables  x(k+l)fv  via  the  functional  relations  of  B(k)» 

~  pxg 

The  same  ntatements  apply  to  the  noise  input  vector  u(k)^>. 

The  reader  should  kcop  in  mind  the  families  of  trajectories  accurately 
described  by  the  J  and  r.  indices,  that  is 

x(k+l^^  «  f(k+l,  k)  x(k)^*  +  B(k)f(k^  +  N(k)u(k^  (3) 

z^(k£>  =  H(k)  x(k>  +  v(k>  (4) 


As  before^ the  accurate  descriptions  designated  by  J  and  n  will  be  dropped 
for  simplicity  of  representation. 

The  equations  are  developed  as  a  "recursive  process"  or  an  "on-line" 
processor;  that  is,  as  the  observations  "role  in"  the  mechanized  computer- 
estimator  utilizes  the  data,  and  discards  it  or  stores  it  on  tape  or  what 
have  you.  All  past  data  is  sequentially  accumulated  in  the  "memory  of  the 
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This  section  derives  the  Kalman  Estimation  Equations  for  the  multivariable 
cue  using  matrix  analysis  methods.  The  derivation  techniques  are  the  same 
as  used  in  the  previous  scalar  case.  The  essential  difference  lies  in  the 
minimization  methods.  The  variance  of  the  estimate  in  state  for  the  scalar 
case  is  a  scalar  valued  function  of  a  scalar  argument  v(k).  For  the  vector 
case,  the  trace  of  the  variance  matrix  of  the  estimate  of  state  is  like¬ 
wise  a  scalar-valued  function,  but  a  function  of  a  matrix  of  p  rows  and 
colunns  Wvk) .  The  minimization  of  a  scalar-valued  function  with  respect 
to  ttys  matrix. 

One  can  arrive  at  the  equations  via  strictly  algebraic  concepts  of  orthogonal 
projection  matrices  etc.,  in  which  one  does  not  have  to  enter  into  discussions 
of  partial  derivaties,  continuity  of  continous  variables  and  gradients. 

Since  the  majority  of  expected  readers  ave  as summed  to  be  more  familiar  with 
the  least-squares  criterion  via  gradients,  thiB  report  will  stick  strictly 
vlth  this  method. 


The  general  linearized  vector  equations  are 


x(k+lHji>«  §(k+l,  k)  x(k)fc>+  B(k)f{k)te>  +  N(k)u(kK^> 
/  /  pxg  /  pxq  ' 

z(k)^>  »  H(k)  x(k)^>  +  v(kjjn> 


The  deterministic  k-varying  vector  1'UOf^  is  of  dimension^g  less  than  or 
equal  to  p,  and  gets  distributed  or  cross-coupled  into  all  p  state 
variables  x(k+l)7j>  via  the  functional  relations  of  B(fc)» 

'  P*e 

The  same  ntatements  apply  to  the  noise  input  vector  u(k)^> . 

The  reader  should  kaop  in  mind  the  families  of  trajectories  accurately 
described  by  the  J  and  r.  indices,  that  is 

x(k+l^^  -  f(k+l,  k)  x(k)Jj>  +  B(k)f(kJ>  +  N(k)uU^>  (; 

zJn(kJ>  =  H(k)  xUj>+  v(k)>  (, 


As  before,  the  accurate  descriptions  designated  by  J  and  n  will  be  dropped 
for  simplicity  of  representation. 

The  equations  are  developed  as  a  "recursive  process"  or  an  "on-line" 
processor;  that  is,  as  the  observations  "role  in"  the  mechanized  computer- 
estimator  utilizes  the  data,  and  discards  it  or  stores  it  on  tape  or  what 
have  you.  All  past  data  is  sequentially  accumulated  in  the  "memory  of  the 
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math-ware"  via  up  dated  estimates  and  variance  matrices  etc. 


Suppose  we  are  at  stage  k  and  have  computed  during  stage  k-1,  the  following 
x(k-l,  k-^£> 


P(k-1,  k-1) 

and  predicted  via  dynamics 


x(k,  k-^>=  |(k,  k-1)  x( k-1,  k-^>+  B(k-l)  f(k^> 
'zCk,  k-l)>=  H(k)  x(k,  k-l£> 

^(k,  k-l}>  «  H(k)  ♦(k,  k-1)  x(k-l,  k-l£>  +  H(k)  B(k-l) 
P(k,  k-1)  =  f(k,  k-1)  P(k-1,  k-1)  fTk,  k-1) 

+  N(k-l)  Q(k-l)  NT(k-l). 


We  now  receive  the  kth  observation 

z(kj^  =  H(k)  x(lO^>  +  v(kj^> 

where  z(k^>  is  known  but  and  v(k^  are  unknown, 

observation  error  vector  by "equation  if))  and  ( ^h)  as 

aT(k,  k-^^  *  H(k)  [x(k£>  -  x(k,  k-1^  ]  +  v(k^ 


are  unknown.  We  can  compute  an 

)  and  (  jjfh)  as 


we  now  can  correct  the  estimate  in  the  state  vector  based  on  the  observable 
and  computable  estimate  in  the  observation  vector  as 

x(k,  k^j^  =  x(k,  k-lj^>  +  W(k)  ^(k,  k-lK^  (H) 

where  the  weighting  matrix  w(k)  at  stage  k  has  p  rows  and  m  columns. 

We  next  seek  a  procedure  for  selecting  attach  stage  a  pxm  weighting 
matrix  W(k). 

Using  equation  (|0)  in  equation  (j|) 

x(k,  *  x(k,  k-^^  +  W(k)  {H(k)[x(k^>  -xkk,  k-1^]  +  v(k^>}  (X2) 

-  [  I  -  W(k)  H(k)]  x(k,  k-l^>  +  W(k)H(k)xUb  ♦  W(k)v(kb> 
pxp  pxm  mxp  /  /  / 
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If  we  now  define  the  "unknown"  error  vectors 


and 

x(k^-  x(k,  k-^^»  x'fk,  k-^^  (14) 

then  theoretically  equation  (L2  )  in  (  13)  yields 

?(*.$>•  »  -  »<k>«k)]x(kj>-  [I  -  W(k)H(k)  ]  x(k,  k->  (15) 

-  W(k)v(k^ 

xU,^^-  [I  -  W(k)H(k)]x(k,  k-^J>-  V(k)v(k£>  (16) 

Transposing  (16)  ve  obtain 

^jfk,  k)  -<^k,  k-l)  [I  -  HT(k)  WT(k)]  -  ^(k)WT(k)  (17) 

The  dyadic  product  of  equation  (  16)  and  (L7  )  yields 

x(k,^><x(k,  k)  -  {[I  «  W(k)H(k)]  “x(k,  k-l£>-  W(k)v(k£»  (18) 

K^(k,  k-l) [I  -  HT(k)  WT(k)]  -<<(k)  WT(k)} 

■«  fl  -  W(k)H(k) ]x(k,  k^<^(k,  k-l) [I  -  HT(k)WT(k)] 

-  [I  -  V(k)H(k)]x(k,  k-lJ><^(k)WT(k) 

-  W(k)v(k)^<^k,  k-l)[l  -  HT(k)WT(k)] 

+  W(k)v(k)]>^(k)WT(k). 

The  square  of  the  magnitude  of  the  error  vector  x^k,  kO  is  given  as  the 
inner-product  of  equation  ( 16  )  and  equation  (17)  or, 'Its  the  trace  of  the 
outer-product  of  equation  ( 18)  as 

<x{k,  k)x“(k,Jt£>-<x(k,  k-l)  tyk,  k-l)>  -  2^(k,  k-l)W(k)rix(k,k-lJ> 
+<[x(k,k-l)HT(k)W,WH(k)  xU,  k-1^  (19) 

-  2^k,  k-l)w(k)  v(k^ 

+  2<^k,  k-l)HT(k)  WT(k)  W(k)  v(k£> 

+  ^(k)  W^(k)  W(k)  v(k^^ 
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Equation  (  19)  is  a  scalar  valued  function  of  a  matrix  argument  W(k)  of 

#■1  70  T"\  Yffl  Uo  ohnl  1  tnlrn  f  Xo  ti  r»  ■**+■  4  *»  1  A  -t  ifn+  4  4>U  a  1 

to  the  matrix  W(k), 


<^(k. 


k)  x(k,  kj>  term  by  term. 


By  equation  (  19) »  there  are  six  additive  terras ,  each  term  will  he  handled 
via  the  "gradient"  methods  in  Appendix  B. 

The  first  term  is  not  a  function  of 

W(k),  hence 


9WfkT  <^^k»  k“1^ 


The  second  term  is  by  equation  fc-61  ) 


3 

3w(k) 


^-2  <^k ,  k- 


1)  W(k)  H(k)  *x(k,  - 


2H(k)x(k 

mcp 


,k-l)^^(k,k- 


1)  (21) 


The  third  term  is 


|j{^x(k,  k-1)  HT(k)  WT(k)  W(k)  H(k)  xtk,  k-l)>} 

*  2H(k)  x(k,  k-l^><^x(k,  k-l)  HT(k)  WT(k) 

The  latter  derivation  is  based  on  equation  fe-80  )  and  setting 
<c  =<^T(k,  k-l)  HT(k) 
b(m>  »  H(k)  **f(k,  k-1^^ 

The  fourth  term  is  by  equation  (b-56) 

{-2^ctk,  k-l)  W(k)  v(k^  )  =  -2v(k^^T(k,  k-l) 

The  fifth  term  by  equation  b-80  is 

~  (2^(k,  k-l)  HT(k)  WT(k)  W(k)  w(k)^> 

■  (H(k)  x(k,  M-lj^X^U)  +  v(k^^tk,  k-l)HT)  WT 
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Koa^^  r\v>  a*>4-  *  <  *->  rr 


,  v(k^>=£s>, 


in  equation  (b-80) 

The  sixth  term  is 

{^(k)  WT(k)  W(k)  v(k^}  -  2v{k^^Uk)  WT(k) 

Utilising  the  above  six  expressions  in  equation  (3.9  )  after  the  partial 
derivative  has  been  taken 

~|^ctk,  k)  x(k,  k^J 

-  -2H(k)  jftk,  k-l^^U,  k-l) 

+  2H(k)‘x(k,  k-lK^acU,  k-l)HT(k)  WT(k) 

-  2v(k^^cU,  k-l) 

+  (H(k)  T(k,  k-l^><v(k)  +  v(k^^tk,  k-l)HT(k)}  WT(k) 

+  2v(kh/v(k)  WT(k)  *  [0] 

'  n  mxp 

The  expected  value  over  all  experiments  and  allowable  values  of  J  and  n 
yields 

Ef^<k'  k)  x(k  k^>J  -  -2H(k)  E{jT(k,  k-l^ctk,  k-l)) 

+2H(k)  E{x(k,  k-l  >x<  k,  k-l)}  HT(k)  WT(k) 

-2E(v(k^^xtk,  k-l)} 

+  [H(k)  E(x(k,  k-l)^>^r{k)  +  v(k^^k,  k-l}  HT(k)]  WT(k) 

+  2E{v(k^^(k)}  WT(k)  -  [0] 
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If  ve  use  the  natation 


P(k,  k)  k)  =  E  {x(k,'^X^k,  k)} 

(31) 

P(k,  k-l)  k~l)  “E{x(k,  k-ljX^k,  k-l) } 

(32) 

Q(k)  *  E{u(k)^<i(k)} 

(33) 

R (k )  »  E  Mk^><v(k)} 

(34) 

and  assume  that  the  conventional  statistical  independence  assumptions  hold. 

E{*(k,  k-l^><^k)>  =  [0] 

(35) 

E(v(k^Xv(k-l)}  *  [0] 

(36) 

E{v(kN<u(k)}  ■  [0],  etc. 

(37) 

By  equation  (  31  )  through  (  37  )  in  equation  (30) 

3{tr  P(k,k) }  «  -2H(k)  P(k,  k-l) 

WIT),  m  m 

+  2H(k)  P(k,  k-l)  H  <k)  W  (k) 

(38) 

+  2^(k)  WT(k)  *  [0] 

[H(k)  P(k,  k-l)  HT(k)  +^y(k)]  WT(k)  *  H(k)  P(k,  k-l) 

(39) 

Transposing 

W(k)[H(k)  P(k,  k-l)  HT(k)  +  2(k)]  =  P(k,  k-l)  HT(k) 

MlU 

(40) 

Inverting 

*  4 

or 

W(k)  *  P(k,  k-l)  HT(k)  [H(k)  P(k,  k-l)  H(k)  +f  (k)J 

1  w 

(41) 

W(k)  *=  P(k,  k-l)  HT(k)[H;(k)  P(k,  k-l)HT(k)  +  R(k)]“a 

(42) 

WT(k)  «  [H(k)  P(k,  k-l)  KT(k)  +  R(k)]'1  H(k)  P(k,  k-l) 

(43) 
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The  pxp  matrix  variance  of  the  estimate  of  state  (the  p-space  ellipsoid  of 
uncertainty)  ****  ho  cbt*i«sd  hy  the  expected  eelue  o',r— i*  ell  AvnA^imAnf 

of  the  dyadic  product  of  equation  (  18) , 

./w.  hv  .  . 

P(k,  k)  »Et(xU,  k^Xjc^k,  k)J  (44) 

■  [I  -  W(k)  H(k)]  P(k,  k-l)[l  -.HT(k).WT(k)] 

♦  W(k)  R(k)  WT(k) 

Multiplying  out  the  terms  of  equation.  (44  )  we  obtain 

P(k,  k)  «  P(k,  k-1)  -  P(k,  k-l)  HT(k)  WT(k)  (45) 

-  W(k)  H(k)  P(k,  k-l)  +  W(k)  H(k)  P(k,  k-l)  HT(k)  WT(k) 

♦  W(k)  R(k)  WT(k) 

■  P(k,  k-l)  -  fi(k,  k-l)  HT(k)  WT(k) 

-  W(k)  H(k)  P(k,  k-l) 

+  W(jt)[H(k)  P(k,  k-l)  HTU)  +  R(k)]  WT(k) 

Consider  the  last  term  of  the  above  equation'  and  equation  (42  )  for  W(k) 
with  the' transpose  (43),  then  the  last  tern  becomes 

W(k)[H(k)  P(k,  k-l)  HT(k)  ♦  R(k) ]  WT(k)  (46) 

■  P(k,  k-l)  HT(k)[H(k)  P(k,  k-l)H^(k)  +  R(k) ]“1(H(k)P(k,k-l)HT(k)+R(k) ] 
x[H(k)  P(k,k-l)  HT(k)  +  R(k)]"1  H(k)  P(k,  k-l) 

«  P(k,k-l)  HT(k)[H(k)  P(k,k-l)  ♦  R(k)]_;L  H(k)  P(k,  k-l) 

■  W(k)  H(k)  P(k,  k-l). 

Using  the  above  expression  for  the  last  term  in  equation  (45)  we  obtain 

P(k,  k)  -  P(k,k-l)  -  P(k,  k-l)  HT(k)  WT(k)  (47) 

-  V(k)  H(k)  P(k,  k-l)  +  W(k)  H(k)  P(k,  k-l) 


P(k,k)  «  P(k,  k-l)  -  P(k,  k-l)  HT(k)  WT(k) 

P(k,k)  »  P(k,  k-l) [I  -  HT(k)  WT(k)] 
pxp  pxm  mxp 

The  matrix  P(k,k-l)  was  predicted  and  computed  during  stage  k-l. 


(48) 

(49) 
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We  can  now  predict  the  next  stage  (k+l)  state  vector  via  the  deterministic 
process-dvnaml ra 

^c(k+l,  k^>=  ♦(k+l,  k)  x(k,  k}>+  B(k)  f(k^  .  (5i 

The  next  stage  prediction  of  the  observation  vector  is 


s(k+l,  H(k+l)  x(k+l,  k£>. 


The  error  in  the  state  vector  at  stage  k+1  basedon  the  prediction  of 
equation  (  50  )  is 


x(k+l,^k^>*  x(k+^^  ■^c(k+l,  k^  (< 

where  the  unknown  state  vector  is 

x(k+lj>«^(k+l,  k)x(k£>  +  B(k)  f(kj>  +  N(k.)  u(kj>  (« 

and  the  unknown  error  is  by  equation  (  50  )  and  equation  (  53  )  in  equation 
(  52  ) 

"at (k+1,  $(k+l,  k)x(k^>  +  B(k)  f(k^  (i 

+  N(k)  u(kj>  -  ♦(k+l,  k)x(k,  kj>'  -  B(k)  f(kj> 

=  *(k+l,  k)[x(k^  »  x(k,^^J>j+  N(k)  u(^^ 

Using  equation  (  I4&  in  equation 

^(k+l,^^*  «(k+l,  k)  “x(k,  k^  +  N(k)  u(k^  (f 

The  transpose  of  equation  ( 55  )  is 

<^(k+l,  k)  *<xtk,  k)  4>T(k+l,  k)  +<u(k)  NT(k)  (! 

The  dyadic  product  of  equation  (55  )  and  equation  (56)  is 
~(k+l,  k+1,  k) 

■  ♦(k+l,  k^k,  k^^(k,  k)^F(k+l,  k) 


>]+  N(k)  u(kj> 
u(k^> 


+  ♦(k+1,  k)0c(k,  k^^^jOO  NT(k) 
+  N(k)  u(^^^x(k,  k)^?(k+l,  k) 

+  N(k)  u(kT><jj(k)  Nr(k) 
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Th*  mraitrt.A+.l r*n  av*»j  i  *»<**«.«*•  4  4. «  _ * j.j  _ ._  /__  \  . 

-  ...._.  — - — — x'  -  *  v*  v^wav^uu  (  /  itJ 

E{x(k+1,  k^X^k+l,  k)}  -  P{k+1,  k)  (5S) 

-  *(k+l,  k)  E{x(k,  k^C^U,  k)}  *T(k+l ,  k) 

*  H(k)  Etu(kj^U)}  NT(k). 

The  statistical  independence  assumption  was  invoked: 

E{x(k,  kj^uOO)  -  [0]. 

Define  the-  process  noise  variance  matrix 
K{u(k^^t(k)}-  Q(k) 
and  equation  (50  becomes 

P(k+1,  k)  -$(k+l,  k)  P(k,  k)<£T(k+l,  k)  +  H(k)  Q(k)  NT(k).  (60) 

We  siy  no*  summarize  the  equations  and  the  computations  to*' be  performed  at 
the  kth  stage  as  the  kth  stage  summary. 

We  have  available  from  stage  k-1  * 

x(k,  k-1^ 

z(k,  k-1^  *  H(k)  'xU,  k-1^* 

P(k,  k-1).  , 

Receive  z(kO 

'  .  >* 

Compute  error  in  observation 

*(k,  z(k^^-  z(k,  k-1^  (61) 

Compute  weight  matrix  W(k)  by  equation  (42) 

W(k)  «  P(k,  k-1)  aT(k)[H(k)  P(k,  k-1)  HT(k)  +  R(k) ]  ^  (62) 

Correct  state  estimate  by  equation  (11) 

x(k,  k^«  x(k,  k-l£>+  W(k)  "z(k,  k-^  (63) 
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(64) 


Compute  new  state  variance  by  equation  (49) 

m  rp 

D/l,  Ir  \  —  p/v  V_1  H  T  _  H  /v\  U  f'lrM 

*  UV  y  »*>  /  *  )  «  -*■  /  L  •  •*  >  ••  /  *•  \  +•»  /  J 


A, 

z(k+l, 


+  B(k)  f(kL 


Predict  to  stage  k+1,  by  equation  (50) 

x(k+l,  k^»  4 (k+1,  k)  x(k,  kj>  +  B(k)  f(kj> 

Predict  observation  by  equation  (51) 

^(k+1,  H ( k+1 )  x(k+l,  k^ 

Predict  state  variance  by  equation  (60) 

P(k+1,  k)  -  *(k+l,  k)  P(k,  k)  4T(k+l,  k)  +  N(k)  Q(k)  NT(k) 
wait  for  stage  k+1  and  new  measurement  vector. 

The  equations  can  be  substituted  and  Juggled  around  to  obtain  alternate 
expressions ^ for  example  using  equation  (42)  and  (61 )  in  (63)  we  obtain 

xU,  k^*  x(k,  k-1^' 

+  P(k ,  k-1)  HT(k)(H(k)  P(k,  k-1 )  HT(k)  +  R(k)]-1 
x{z(k^  -H(k)  «(k,  k-1)  x(*-l,  k-lj> -H(k)B(k-l)f  (k-l£>  ) 

Many  similar  variations  of  the  above  systems  of  equations  occur  in  the 
literature. 

Stage  k  -  1. 

The  vector  starting  system  of  equations  can  be  derived  from  equation 
for  k  =  1, 


x(l,  «*  intelligent  guess 
'zd.O^  =  H(l)  xU,o£> 

P(l,  0)  =  intelligent  guess  based  on  experience  about  the  process. 


Receive  z(l) 


Compute  error 


B(lj> 

error 

,  C^=  z^^^  -  z(l,  0)> 
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+  ffht. 

V(l)  -  P(l,  0)  [HT(l)  P(l,  0)  ht(i)  +  R(1)]_1 

(73) 

Correct  state 

x(l,  lj>  «  x(l,^oJ>  +  W(l) 

(74) 

Compute  state  variance  matrix  by  equation  49 

P{1,  1)  -  P(l,  0)  [I  ^  ht(i)  wt(i)] 

(75) 

Predict  stage  2  by  equation  (50  ) 

/x(2,  l^»-<£(2,  1)  x(l,  ^]>  +  B(l)  f(lj> 

(76) 

Predict  stage  2  observation 

/t(2,  H(2)  x(2, 

(77) 

p(2,  l)  -  «(2,  i)  p(i,  i)  fce<2,  l)  -*•  NU)q(l)Nl(l) 

(78) 
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IV.  EQUATION  SUMMARY  FOR  COMPUTER  APPLICATION 


This  section  summarizes  the  equations  of  the  previous  sections  anu  points 
out  how  to  compute  mechanize  the  estimation  equations  to  recursively  estimate 
the  state  vector  es  the  observations  "roll  into  the  computer".  Precomputation 
of  the  sequence  of  weighting  matrices  for  large  dynamical  systems  is  a  necessity. 

The  dynamical  process  is  described  by  the  state  vector  equation 


X(k+1>  -  4>(k+l,  k)X(k>  +  Bf(k>  +  Bf(k>  +  N(k)  U(k> 


(1) 


and  a  system  of  noisy  instruments  whose  outputs  are  functionally  related  to 
the  states  by  the  observation  equation 


zjk>  -  HoOX(k>  +  v(k>  . 

The  system  of  estimation  equations  are: 
The  state  vector  prediction  equation 


(2) 


X(k+1 


$  (k+1 ,  k)X(k^k>  +  B(k)f(k>. 


The  observation  prediction 


Z(k+1 


•> 


H  (k+1)  X  (k+1 


> 


The  Observation  error 


and  the  correction  to  the  predicted  states  at  k+1  after  the  (k+1) 
observation  is  available 


(3) 


(4) 


(5) 


X(k+1 


,  k+l> 


X(k+1 


+  W(k+1) 


(6) 


The  sequence  of  weighting  matrices  W(k)  can  be  precomputed  and  stored  in 
memory.  The  weights  are: 
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W(k+1)  -  P(k+1,  k)H(k+l) 


^(k+I)P(k+l,  k)H  (k+1)  +  R(k+1 


(7) 


where 


P(k+l,  k)  -  #(k+l,k)P(k,k)$T(k+l,  k)  +  N(k+l)Q(k+l)NT(k+l)  .  (8) 


The  block  diagram  is  shown  in  Figure  (1)  as  the  conventional  feedback 
(discreet)  system. 


FIGURE  (1)  -  DISCREET  FEEDBACK  BLOCK 


By  re-arranging  the  positions  of  the  feedback  blocks  one  can  obtain  a 
flow-block  which  looks  more  familiar  to  a  digital  programmer  as  shown  In 
Figure  (2) . 

Tests  or  applications  in  which  one  can  plan  or  design  the  experiment  and 
the  times  k,  k+1,  etc.  at  which  instrument-data  will  be  used  to  estimate 
appear  to  admit  of  pre-computing  the  weights.  If  the  estimation  times  are  not 
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APPENDIX  A  -  MATRIX  TRACE  PROPERTIES 


The  trace  of  a  matrix,  the  trace  of  the  product  of  tvo  matrices, 
and  the  trace  of  a  matrix-sum  ar#  useful  notions  to  aid  the  development 
of  the  topics  of  Appendix  B.  v 

Consider  a  matrix^  of  p  rows  and  a  columns  where  m  <  p  and  a 

matrix  B  ,  then  the  product 
raxp 


Q,  -  A  B 
px^i  P*»  ““P 

is  a  pxp  matrix. 

The  matrices  A  and  B  can  be  partitioned  into  their  row  and  coluan 
spaces  as  shpvn 


(1) 


A  ■ 


m  m 

ft  ♦  •  ♦ 

■  m 


$ 


b(^>, 


L 

“V 


The  product  Q  can  be  written  as  a  matrix  of  inner-products 


*  AB  * 


<nja 

e 

k*J 

b  Cjp 

e 

I 

|<^1a  b^j^ 


■  •  .  b(n£>| 


•  ^Ta  b(^ 


(2) 


(3) 


00 


(5) 
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Equation  (7)  expresses  as  a  sum  of  m  rank-one  matrices. 

If  we  commute  t Lie  product  we  obtain  a  square  mxm  matrix 

Q0  -  3  A 

2  mxp  pxm 


and  as  before  Q2  can  be  written  as  a  matrix  of  inner-products 


(6) 


(7) 


(8) 


(9) 


(10) 


(11) 
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Clearly  matrix  multiplication  is  not  commutative ,  that  is 

AB  4  »A 
pxp  1  mxm 

in  fact  the  matrices  are  not  even  of  the  same  size. 


/  ■■  ~ 
\  xc, 


\ 

/ 


However  the  trace  of  both  products  are  equal,  that  is 


tr(A£ 


The  following  will  clarify  the  above  relation. 


If  we  have  a  column  vector  x(j^  and  a  row  ve 
dimension  p  then  the  dyadic  product  is  the  square 
and  p  columns  ,  ,  „ 


ctor  ^Ty  o 
,  raniwbne, 


of  the  same 
,  matrix  D  of  p  rowi 


-  ‘>4-  -  (V1 '  ■  ■ 

•  •  ■  •  v 


If  we  commute  the  product  of  Equation  (lU)  we  obtain 


“  yjX1 


+  y,x  + 


+  y  xF 
P 


a  scalar. 


When  the  elements  y^  and  x  are  real  field  elements  the  products 
commute,  hence 


and  Equation  (15)  [the  inner  product]  can  be  written  as  the  sum  of  the  main 
diagonal  terms  of  }£><£>  which  the  conventional  definition  of  the  t  -ace  (tr)  of 
a  matrix,  hence 


The  dyadic  product  is  not  as  mysterious  as  many  novices  might 
imagine;  in  fact,  if  we  write  Equation  (l*t)  as 


we  see  that  the  matrix  0  when  partitioned  into  its  column  space  is  a  row 
of  P  parallel  column  vectors  -  all  p  of  the  vectors  lie  on  a  line,  hence 
'&><£  is  said  to  have  rank  one  -  that  is,  there  is  only  one  linearly 
Independent  vector  in  the  row  "package"  of  column  vectors. 


y><k  -  j>k»  y2  •  •  •  yJ 

-[>  yl>  >y2  *  *  \>y^j 

matrix  0  when  partitioned  inti 
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APPENDIX  B 

GRADIENTS  OF  SCALARS  WITH  RESPECT  TO  MATRICES 


This  appendix  develops  the  gradient  of  a  scalar-valued  function  with 
respect  to  a  vector  variable  and  also  with  respect  to  a  matrix  variable. 

Case  1.  q  =  <^)a  x(jg>.  Consider  the  scalar  q  which  is  the  inner- 

product 

q  -  <£)a  x(^  (b-1) 

where  ^  ia  a  fixed  p  dimensional  row  vector  and  1b  a  variable  column 

vector,  or  q  is  said  to  be  a  scalar-valued  variable  which  is  a  function 
of  the  vector  variable  ^ . 

In  equation  (b-l)  q  may  be  considered  to  have  vector  factors  and^. 


If  we  have  a  dyad 

Q  »  ^ 

then  it  warn  shown  in  appendix  A  that 
tr  Q  ■  q 


(b-2) 


(b-3) 


tr  H  =  ^  ^  =  * 

The  differential  of  equation  (b-2)  is 
d  Q  *  d  ?--  ^ 


(b-5) 


and  the  trace  of  (b-5)  is 


tr  dQ  ■  tr  [jj>  <fj=  <  d>  =  dq 


We  iqay  now  ask  to  express  the  differential  matrix  dQ  in  terms  of  vector 
factors  d»  and  a  gradient  vector,  that  is 


such  that  the  trace  of  equation  (b-7)  is 


dq  »  tr  dQ  «  tr 


(b-8) 
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If  we  take  the  trace  of  AB  by  Equation  (  ’j)  as  the  sum  of  diagonals 
we  obtain 

m«jj;  »  N[i)a  DUP'  + .  +<5n)a  b(np  (12) 


If  we  take  the  trace  of  dyadic  sum  decomposition  of  Ah  given  by 
Equation  (  7)  we  obtain 

a(R>  <^)b  + 


tr(AB)  “  tr 


*  ^ 


+  a(^><^Tb 


(13) 


Tlie  trace  of  a  sum  of  matrices  is  the  sum  of  the  traces,  hence  by 
Equation  ( 17) 

tr(AB)  -  tr  a(g><£)b  +  tr  + .  +  tr  a^X^b  (11*) 


jj><£)b  +  tr  a^><^)t 
tr(AD)  •  <^)b  a(^  +  <|)b  +••••••’  +  <^)b  a(p> 


(15) 


Equation  U-2)  is  a  sum  of  p  inner-products  of  m-dimensional  vectors  and 
Equation  (1'5)  is  a  sum  of  m  inner-products  of  p-dimensional  vector. 


The  sum  of  the  main  diagonal  terms  of  Equation  (y  )  is 


tr(l)A) 


<^b  aXy>  + . . 


which  by  Equation  (15)  and  Equation  (in) 
tr(Ab)  -  trfcSA} 


(16) 


(17) 


By  equati  n  (b-7)  and  (b-5)  we  can  state 


*4 

We  arrive  at  the  result  of  equation  (b-9)  directly  from  (l) 

dq  -  »<^  ^ 


hence 


AIbo  one  can  consider  the  gradient  as  an  operator 


i<^  ■  '4^] 


(b-9) 

(b-10) 

(b-11 ) 

(b-12) 


The  dyadic-type  operator 


when  the  coordinates  are  independent  of  each  other,  then 


(b-13) 


(b-llt) 


(b-15) 
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Hence 


A  yl 
*V^  <4 


(b-16) 


In  conclusion: 


(b-17) 

(b-18) 


When  q  is  quadratic  ve  can  write  q  as  the  trace  of  the  dyad 


Q  3 

(b-19) 

for 

tr  Q  =  tr  3  ^  a  q. 

C  b— 20) 

The 

differential  of  the  dyad 

dQ  ■  dj>^  + 

(b-21) 

dq  -  tr  dQ  -  <£>  +  ^>  =  2  ^ 

(b-22) 

hendo 

<(S  »  2< 

(b-23) 

Case  3. 

q  ®  B  ^ 

For 

this  case  we  have  two  different  matrices 

(b-25) 

and 

(b-26) 
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which  under  the  trace  operation  map  down  to  the  same  scalar 

/  V 


5  «  tr  *  tr  q2  «  ^  B  y 

(b-27) 

The  di  t ferent.1  a!  of  Q .  *  0  i  s 

■ 

dQ  *  d£>^B  +  ^<$lxB 

(b-28) 

The  trace  of  (b-28)  is 

tr  dQ  =  ^  B  +  <^x  Bj> 

(b-29) 

The  differential  of  (b-24)  is 

dq  »  B^>  +  ^B  <b£>  =  tr  Q 

(b-30) 

dq  *  <BT  <j^>  +  <$eB  d£> 

(b-31) 

d-q.  «  <£  ju  +  B^J  d^ 

(b-32) 

we  have  s 

(b-33) 

and  by  (b-32)  and  (b-33) 

4-<[,+,T] 

(b-3>0 

and  for  symmetric  B 

T 

B  -  B1 

(b-35) 

then  . 

(1-36) 

Case  U.  q  * 

(b-3Y) 

The  scalar  q  is  a  function  of  the  matrix  X  of  p-rows  and  m  columns. 
The  scalar  q  can  be  written  as  the  trace  of  the  matrix 


0>-38) 


The  differential  of  Q  iB 

dQ  ■><  dX  (b-39) 

By  equation  (b-37).  differentiating 

dq  - ^ dX -  tr  dQ.  (b~40) 

We  seek  a  gradient  matrix  3<q  of  m  rows  and  p  columns  as  one  of  the  factors 

3X 

of  dQ  that  is 


dQ  =  3^  dX  (b-kl) 

axm  Jx  pen 
nup 


such  that 


trdQ  =  dq  =*  <^dxy> 

Clearly  by  equation  (b-39)  and  (b~4l)  if 


Is. 

ax 

sup 


=  b(*j>^£ 


then  (b-42)  is  satisfied. 


(b-l*2) 


(b-U3) 


a 


An  alternate,  more  direct,  approach  is  given  below.  Partition  X  into 
row  of  column  vectors  (all  "  contravariant- "  vectors),  then 


Cb-uu) 
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(b-lfj) 


(b-H?) 


(b-ltb) 


(b-i.9) 
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The  differential  of  X  is  a  row  of  column  vectors 


and  the  gradient  matrix  is  a  column  of  row  gradient-vectors . 


(b-50) 


la 

3X 


(b-51) 


1*1 

From  the  foregoing  we  write 

dQ  *>  3^  dX 

3X 

mxp 


and 


By 


dq  <*  tr  dQ  =  tr 
equation  (b-45)» 


J  3^  dX  j 

L«  J 

(b-l*6)  and  (b-l6) 


(b-52) 


(b-52) 


(b-53) 


Packaging  the  row  vector  gradients  of  (b-53)  into  the  column  of  (b-5l) 
we  obtain 


ia  s  I 

3X 

b2<^)a 

• 

at 

f  b1^ 

?  ! 

b'  1 
• 

(b-5U) 

bm<^)a 

v  J 

la.  .  k^< 

ax  ~'r 

nxp 


hence  in  conclusion 


if  q  ■  ^)a  X  b(j^ 

pxm 

then  ]l£ 

ax 

<d 

V 

A 

II 

mxp 

_ 

C/me  5 .  q  =  <p)a  X  B  n(h> 

- —  ^  pxm  mxp  yr 


For  this  case  we  set 


B 

mxp 


a(j}>  =  b{jj> 

as  in  equation  (b-56),  then 

q  =  <£  X  b(m> 
pxm  x 

and  we  obtain  the  case  fc,  hence 


ax 


mxp 


a(j)><^)  f 


or 


if  1  - 

then  H  -B  *(><)• 
mxD  ^  ^ 


mxp 


\  *•  //  > 


(b-56) 


(b-57) 


(b-58) 


(b-59) 


(b-6o) 


(b-6.1) 
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Case  6. 


vu-u^; 


! 

! 


*£)a  X  XTb?p>. 
■s  pxra  mxp  ' 


This  case  Is  the  matrix  analog  of  the  quadratic  vector  cose  of  equation 

(b-2»0. 

T 

We  can  partition  X  into  its  colwnn  space  and  X  into  its  row  space  and 
obtain 


,^)a  •  ‘  •  ">] 


(b-63) 


and 


<e*J 

^|^x(^>^)x  +  ...■*•  x(^^)xj  b(^ 


(b-6U) 


Distributing  the  two  end  vectors  over  the  dyadic-sura  decomposition 
of  XXT  we  obtain 

q  *  ...  + 

x  ^  2  a 

Because  of  inner-product  commutativity 

i. 


hence 


(b-65) 

(b -66) 

(b-67) 


m  ra 


P1^>)  +  •  •  •  +  Pm(^) 


ra  m 


hence  the  scalar  q  is  a  sura  of  products  of  scalars  p^  q^. 
We  have  as  before 


a*  a  21  41  i! 

dq- "  S  »»••••  te  J 

m*  " 


4> 


* 


tr  dQ 


(b-68 ; 
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■'nWvAilM,1,, 


(b-69) 


(b-70) 

(b-71) 

(b-72) 

(b-73) 

(b-7!0 

(b-75) 

(b-76) 


Consider  the  pxp  matrix  L  which  has  factors  as  shown 


L  «  B  .X 

pxp  pxk  kxp 

where  X  is  a  variable  matrix. 

If  ve  factor  fi  into  its  column  space  and  X  into  its  rov  space 


(b-8l) 


(b-82) 


(b-83) 


The  trace  of  L  is 

1  k 

tr  L  *  A  *  •  •  .  ♦  (b-84) 

1  k 

Hie  differential  of  (t>4  is  * 

dL  «  B  d  X.  (b-05) 

The  factors  of  dL  can  also  be  expressed  as 


dL  ■ 


where  the  pxk  gradient  matrix  is 


(b-86) 


(b-OT) 


APPENDIX  C 


MINIMIZATION 


Consider  the  linear  surface 


1 

and  the  quadratic  surface 

q  -<xQx> 


(1) 

(2) 


and  the  difference 


q  -  A  ■  (3) 

If  £  is  a  constant,  A  «  to,  then  we  seek  a  vector  xSthat  lies  on 
the  linear  surface  and  on  the  quadratic  surface  such  tint  difference  in 
the  linear  surface  and  the  quadratic  surface  is  a  minimum. 


Differentiating 
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\A 

By  equation  r  )  end  equation  r  ) 


and  solving  tor^x 


Multiplying  equation  (11)  by  ^  and  using  equation 
<^b)^D.q-_1b>-  to 

1  to 

?  <^bQ_1b> 

Using  (13)  in  (11) 


If 


then 


>*St  ■  to  &  Q"1 

^  <bQ-Xb>  • 


to  »  i 


(10) 

(11) 

(12) 

(13) 

(HO 

(15) 
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